#ifndef __SE3MATH_H__
#define __SE3MATH_H__
#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#ifndef PI
#define PI 3.1415926
#endif

typedef struct{
    float m[4][4];
} Matrix;

typedef struct{             //  Modeified D_H
    float init_theta;       //  joint angle
    float d;                //  joint offset
    float a;                //  link length
    float alpha;            //  link twist
} DH;

/*
    (x, y, z) represent positions,
    (qx, qy, qz, qw) represent quaternions;
*/
typedef struct{
    float x;
    float y;
    float z;
    float qx;
    float qy;
    float qz;
    float qw;
} Pose;

/*
    (qx, qy, qz, qw) represent quaternions;
*/
typedef struct{
    float qx;
    float qy;
    float qz;
    float qw;
} Quaterion;

typedef struct{
    float x;
    float y;
    float z;
} Point;

/*
    there are many definitions of Euler angles;
    we use external rotation method and X-Y-Z order for rotation;
*/
typedef struct {
    float rx_angle;
    float ry_angle;
    float rz_angle;
} Euler;

//  4*4 Matrix
void generateZeroMatrix(Matrix* M);
void generateDiagonalMatrix(float Diag_value, Matrix* M);
void rotateXMatrix(float angle, Matrix* M);
void rotateYMatrix(float angle, Matrix* M);
void rotateZMatrix(float angle, Matrix* M);
void matrixEqual(Matrix* A, Matrix* B);
Matrix matrixMultiply(Matrix* A, Matrix* B);
float matrixDetValue(Matrix* M);
//  DH
void DH2Matrix(DH dh, float joint, Matrix* M);
//  Quaterion
void quat2Matrix(Quaterion* quat, Matrix* M);
void matrix2Quat(Matrix* M, Quaterion* quat);
float quatNorm(Quaterion* quat);
float quaternionDotProduct(Quaterion* quat1, Quaterion* quat2);
void quaternionProduct(Quaterion* quat1, Quaterion* quat2, Quaterion* quat);
void quaternionAdd(Quaterion* quat1, Quaterion* quat2, Quaterion* quat);
void quaternionMultConstant(Quaterion* quat1, float multiplier, Quaterion* quat);
void quaternionSlerp(Quaterion* quat1, Quaterion* quat2, float t, Quaterion* quat);// Slerp: an interpolation method
//  Pose
void pose2Matrix(Pose* pose, Matrix* M);
void matrix2Pose(Matrix* M, Pose* pose);
void getQuatFromPose(Pose* pose, Quaterion* quat);
void getPointFromPose(Pose* pose, Point* point);
//  Euler
void euler2Matrix(Euler* euler, Matrix* M);
void matrix2Euler(Matrix* M, Euler* euler);
//  Point
void getMiddlePoint(Point* point1, Point* point2, float t, Point* mid_point);
void quatPoint2Pose(Quaterion* quat, Point* point, Pose* pose);

#endif